import serial
import time
import ctypes

###################################################################################################
# 德晟智能科技总线舵机通信协议
###################################################################################################
SERVO_FRAME_HEADER = 0xF9, 0xFF
SERVO_CMD_MOVE_TIME_WRITE = 0x03  # 写指令，往“舵机控制指令地址列表”写入数据
SERVO_CMD_MOVE_TIME_READ = 0x02  # 读指令，读取“舵机控制指令地址列表”数据
SERVO_CMD_ID_WRITE = 0x0F  # 舵机ID写入指令
SERVO_CMD_ID_READ = 0x0F  # 舵机ID读取指令
SERVE_MOVE_TIME_WRITE = 0x65  # 舵机控制运动代码指令值
ROBOT_SERVO_MOVE_STOP = 0x02  # 舵机停止指令
ROBOT_SERVO_ID_READ = 0x0F  # 读取舵机的ID值,改id需要输入旧id和新id
ROBOT_SERVO_LOAD_OR_UNLOAD_WRITE = 0x00  # 设置舵机内部电机是否掉电
SERVO_CMD_MOVE_STOP = 0x64
ROBOT_SERVO_POS_READ = 0x46  # 读取舵机当前实际角度位置值
serialHandle = serial.Serial("/dev/ttyS3", 115200)  # 初始化串口，波特率为115200
time_out = 50


def map_angle_new(old_angle):
    """将老舵机的角度映射到新舵机的角度"""
    # 老舵机角度范围：0到270度
    # 新舵机角度范围：-1800到1800度
    new_angle = (old_angle / 1000) * 2400 - 1350
    #print(new_angle)
    return int(new_angle)


def map_angle_old(new_angle):
    """将新舵机的角度映射到老舵机的角度"""
    # 老舵机角度范围：0到270度
    # 新舵机角度范围：-1800到1800度
    old_angle = (new_angle + 1350) / 2400 * 1000
    #print(old_angle)
    return int(old_angle)


def checksum(buf):
    """计算校验和"""
    check_sum = 0x00
    for b in buf:
        check_sum += b
    check_sum = check_sum - 0xF9 - 0xFF
    check_sum = ~check_sum  # 取反
    return check_sum & 0xff


def serial_servo_write(ID, cmd, addr, dat1=None, dat2=None):
    """写指令"""
    buf = bytearray(SERVO_FRAME_HEADER)  # 帧头
    buf.append(ID)
    # 指令长度
    if dat1 is None and dat2 is None:
        buf.append(3)
    elif dat1 is not None and dat2 is None:
        buf.append(4)
    elif dat1 is not None and dat2 is not None:
        buf.append(7)

    buf.append(cmd)  # 指令
    buf.append(addr)
    # 写数据
    if dat1 is None and dat2 is None:
        pass
    elif dat1 is not None and dat2 is None:
        buf.append(dat1 & 0xff)
    elif dat1 is not None and dat2 is not None:
        buf.extend([(0xff & dat1), (0xff & (dat1 >> 0x08))])  # 分低8位 高8位 放入缓存
        buf.extend([(0xff & dat2), (0xff & (dat2 >> 0x08))])  # 分低8位 高8位 放入缓存
    # 校验和
    buf.append(checksum(buf))
    #print(buf.hex())
    serialHandle.write(buf)  # 发送


def serial_servo_read(ID, r_cmd):
    """发送读取命令"""
    buf = bytearray(SERVO_FRAME_HEADER)  # 帧头
    buf.append(ID)
    buf.append(3)  # 指令长度
    buf.append(SERVO_CMD_MOVE_TIME_READ)
    buf.append(r_cmd)  # 指令
    buf.append(checksum(buf))  # 校验和
    # print(buf.hex())
    serialHandle.write(buf)  # 发送
    time.sleep(0.00034)


def serial_servo_get_r_msg(cmd):
    """获取指定读取数据命令的数据"""
    serialHandle.flushInput()  # 清空接收缓存
    time.sleep(0.005)  # 稍作延迟，等待接收完毕
    count = serialHandle.inWaiting()  # 获取接收缓存中的字节数
    if count != 0:
        recv_data = serialHandle.read(count)  # 读取接收到的数据
        try:
            if recv_data[0] == 0xF9 and recv_data[1] == 0xF5 and recv_data[5] == cmd:
                dat_len = recv_data[3]
                serialHandle.flushInput()  # 清空接收缓存
                if dat_len == 4:
                    return recv_data[6]
                elif dat_len == 5:
                    pos = 0xffff & (recv_data[6] | (0xff00 & (recv_data[7] << 8)))
                    return ctypes.c_int16(pos).value
                elif dat_len == 7:
                    pos1 = 0xffff & (recv_data[6] | (0xff00 & (recv_data[7] << 8)))
                    pos2 = 0xffff & (recv_data[8] | (0xff00 & (recv_data[9] << 8)))
                    return ctypes.c_int16(pos1).value, ctypes.c_int16(pos2).value
            else:
                return None
        except BaseException as e:
            print(e)
    else:
        serialHandle.flushInput()  # 清空接收缓存
        return None


###################################################################################################
# 舵机使用API
###################################################################################################
def setBusServoID(old_id, new_id):
    """
    配置舵机id号, 出厂默认为1
    :param old_id: 原来的id， 出厂默认为1
    :param new_id: 新的id
    """
    serial_servo_write(old_id, SERVO_CMD_ID_WRITE, new_id)


def getBusServoID(ID=None):
    """
    读取串口舵机id
    :param id: 默认为空
    :return: 返回舵机id
    """
    while True:
        if ID is None:  # 总线上只能有一个舵机
            serial_servo_read(0xfd, SERVO_CMD_ID_READ)
        else:
            serial_servo_read(ID, SERVO_CMD_ID_READ)
        # 获取内容
        msg = serial_servo_get_r_msg(SERVO_CMD_ID_READ)
        if msg is not None:
            return msg


def setBusServoPulse(ID, pulse, use_time):
    """
    驱动串口舵机转到指定位置
    :param id: 要驱动的舵机id
    :param pulse: 位置
    :param use_time: 转动需要的时间
    """
    pulse = int(map_angle_new(pulse))
    pulse = -1800 if pulse < -1800 else pulse
    pulse = 1800 if pulse > 1800 else pulse
    use_time = 0 if use_time < 0 else use_time
    use_time = 30000 if use_time > 30000 else use_time
    serial_servo_write(ID, SERVO_CMD_MOVE_TIME_WRITE, SERVE_MOVE_TIME_WRITE, pulse, use_time)


def getBusServoPulse(ID):
    """
        读取舵机当前位置
        :param id:
        :return:
        """
    while True:
        serial_servo_read(ID, ROBOT_SERVO_POS_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_POS_READ)
        if msg is not None:
            msg = map_angle_old(msg[0])
            return msg
        else:
            return 0


def stopBusServo(ID):
    """
    停止舵机运行
    """
    serial_servo_write(ID, SERVO_CMD_MOVE_TIME_WRITE, SERVO_CMD_MOVE_STOP, ROBOT_SERVO_MOVE_STOP)


def ServoPowerOff(ID):
    """
    舵机关闭扭矩
    """
    serial_servo_write(ID, SERVO_CMD_MOVE_TIME_WRITE, SERVO_CMD_MOVE_STOP, ROBOT_SERVO_LOAD_OR_UNLOAD_WRITE)


def ServoPowerOn(ID):
    """
    舵机开启扭矩
    """
    serial_servo_write(ID, SERVO_CMD_MOVE_TIME_WRITE, SERVO_CMD_MOVE_STOP, ROBOT_SERVO_MOVE_STOP)


###################################################################################################
# 主程序
###################################################################################################
if __name__ == "__main__":
    # 示例：控制2号舵机在1000ms内到达1000位置
    #ServoPowerOn(2)
    # time.sleep(2)
    setBusServoPulse(2, 400, 300)
    time.sleep(1.5)  # 等待舵机完成动作
    data = getBusServoID()
    print(data)
    position = getBusServoPulse(2)
    # ServoPowerOff(2)
    print(f"舵机2当前位置：{position}")
    #serialHandle.close()  # 关闭串口
